#include <Eigen/Dense>
#include <iomanip>
#include <iostream>
using namespace std;
using namespace Eigen;

/**
 * @brief colPivHouseholderQr() 会自动将线性相关列对应的解设为0
 * 与去除线性相关列后的计算效果一样,若同为线性相关项,会去除后面的列
 * @return int
 */

int main() {
  cout << setprecision(15) << scientific;
  MatrixXd A(4, 3);
  A << 1, -2, 1, -1, 3, 0, 2, 1, 7, 3, -2, 7;
  cout << "rank = " << A.colPivHouseholderQr().rank() << endl;
  // threshold程序内部会自己进行设置一个值
  // When it needs to get the threshold value, Eigen calls threshold(). By
  // default, this uses a formula to automatically determine a reasonable
  // threshold. Once you have called the present method setThreshold(const
  // RealScalar&), your value is used instead.
  cout << "threshold = " << A.colPivHouseholderQr().threshold() << endl;
  cout << "R = " << A.colPivHouseholderQr().matrixR() << endl;

  cout << "Here is the matrix A:\n" << A << endl;
  VectorXd b = VectorXd::Random(4);
  cout << "Here is the right hand side b:\n" << b << endl;
  cout << "The least-squares solution is:\n";
  VectorXd c = A.bdcSvd(ComputeThinU | ComputeThinV).solve(b);
  cout << c << endl;
  double err, relative_err;
  err = (A * c - b).norm();
  relative_err = err / b.norm();
  cout << "err = " << err << "  relative_err = " << relative_err << endl;

  //   b = VectorXd::Random(4);
  cout << "Here is the right hand side b:\n" << b << endl;
  cout << "The least-squares solution is:\n";
  VectorXd d = A.colPivHouseholderQr().solve(b);

  cout << d << endl;
  err = (A * d - b).norm();
  relative_err = err / b.norm();
  cout << "err = " << err << "  relative_err = " << relative_err << endl;

  auto B = A.colPivHouseholderQr();
  cout << B.matrixR() << endl;
  cout << B.colsPermutation().indices() << endl;
  // 去除多余的列
  MatrixXd C(4, 4);
  C << -2, 1, -2, 1, 3, 0, 3, 0, 1, 7, 1, 7, -2, 7, -2, 7;
  cout << "Here is the matrix C:\n" << C << endl;
  cout << "Here is the right hand side b:\n" << b << endl;
  cout << "The least-squares solution is:\n";
  VectorXd e = C.bdcSvd(ComputeThinU | ComputeThinV).solve(b);
  cout << e << endl;
  err = (C * e - b).norm();
  relative_err = err / b.norm();
  cout << "err = " << err << "  relative_err = " << relative_err << endl;

  Eigen::ColPivHouseholderQR<Eigen::MatrixXd> D = C.colPivHouseholderQr();
  VectorXd f = D.solve(b);
  cout << f << endl;
  err = (C * f - b).norm();
  relative_err = err / b.norm();
  cout << "err = " << err << "  relative_err = " << relative_err << endl;
  cout << D.colsPermutation().indices() << endl;
  cout << "R = \n" << D.matrixR() << endl;
  cout << "C = \n" << C << endl;

  // cout << C.colPivHouseholderQr().householderQ().setLength(
  //             C.colPivHouseholderQr().nonzeroPivots())
  //      << endl;
}